// Created by Chengfu Zou
// Maintained by Chengfu Zou, Labor
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "armor_solver/armor_solver.hpp"
// std
#include <cmath>
#include <cstddef>
#include <stdexcept>
// project
#include "armor_solver/armor_solver_node.hpp"
#include "rm_utils/logger/log.hpp"
#include "rm_utils/math/utils.hpp"

namespace fyt::auto_aim {
Solver::Solver(std::weak_ptr<rclcpp::Node> n) : node_(n) {
  auto node = node_.lock();

 // 声明并获取参数
  shooting_range_w_ = node->declare_parameter("solver.shooting_range_width", 0.145);//射击范围的宽度
  shooting_range_h_ = node->declare_parameter("solver.shooting_range_height", 4.0);//射击范围的高度
  max_tracking_v_yaw_ = node->declare_parameter("solver.max_tracking_v_yaw", 6.0);//最大跟踪速度偏航角
  prediction_delay_ = node->declare_parameter("solver.prediction_delay", 0.0);//预测延迟
  controller_delay_ = node->declare_parameter("solver.controller_delay", 0.0);//控制器延迟
  side_angle_ = node->declare_parameter("solver.side_angle", 15.0);//侧向角度
  min_switching_v_yaw_ = node->declare_parameter("solver.min_switching_v_yaw", 1.0);//最小切换速度偏航角

  // 声明并获取参数S
  std::string compenstator_type = node->declare_parameter("solver.compensator_type", "ideal");
  trajectory_compensator_ = CompensatorFactory::createCompensator(compenstator_type);
  trajectory_compensator_->iteration_times = node->declare_parameter("solver.iteration_times", 20);
  trajectory_compensator_->velocity = node->declare_parameter("solver.bullet_speed", 20.0);
  trajectory_compensator_->gravity = node->declare_parameter("solver.gravity", 9.8);
  trajectory_compensator_->resistance = node->declare_parameter("solver.resistance", 0.001);

  normal_v = trajectory_compensator_->velocity;

  state = State::TRACKING_ARMOR;
  overflow_count_ = 0;
  transfer_thresh_ = 5;

  last_idx_ = -1;           // 上次选板索引
  hold_count_ = 0;          // 连续保持计数

    // 定义最大转速、最小转速、最大角度、最小角度等参数
  max_v_yaw = 12.0; // 最大敌方转速
  min_v_yaw = 1.5;  // 最小敌方转速

  last_fire = false;




  node.reset();
}

rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &target,
                                            const rclcpp::Time &current_time,
                                            std::shared_ptr<tf2_ros::Buffer> tf2_buffer_,
                                          double&want_yaw,double&want_pitch,double &min_limit_angle,double &max_limit_angle,
                                        int &index,std::vector<Eigen::Vector3d> &fire_target,std::vector<Eigen::Vector3d> &shoot_target) {
  // Get newest parameters
  try {
    auto node = node_.lock();
    max_tracking_v_yaw_ = node->get_parameter("solver.max_tracking_v_yaw").as_double();
    prediction_delay_ = node->get_parameter("solver.prediction_delay").as_double();
    controller_delay_ = node->get_parameter("solver.controller_delay").as_double();
    side_angle_ = node->get_parameter("solver.side_angle").as_double();
    min_switching_v_yaw_ = node->get_parameter("solver.min_switching_v_yaw").as_double();
    node.reset();
  } catch (const std::runtime_error &e) {
    FYT_ERROR("armor_solver", "{}", e.what());
  }

  // Get current roll, yaw and pitch of gimbal
  std::array<double, 3> rpy;
  try {
    auto gimbal_tf =
      tf2_buffer_->lookupTransform(target.header.frame_id, "gimbal_link", tf2::TimePointZero);
    auto msg_q = gimbal_tf.transform.rotation;

    tf2::Quaternion tf_q;
    tf2::fromMsg(msg_q, tf_q);
    tf2::Matrix3x3(tf_q).getRPY(rpy[0], rpy[1], rpy[2]);
    rpy[0] = -rpy[1];
  } catch (tf2::TransformException &ex) {
    FYT_ERROR("armor_solver", "{}", ex.what());
    throw ex;
  }

  // Use flying time to approximately predict the position of target
  Eigen::Vector3d target_position(target.position.x, target.position.y, target.position.z);
  double target_yaw = target.yaw;
  /*
  auto node = node_.lock();
  if (node) {
    double bullet_speed{};
    // 从参数服务器中读取当前值（若未设置，则 bullet_speed 保持为 0 或你指定的初始值）
    if (node->get_parameter("solver.bullet_speed", bullet_speed)) {
      if(bullet_speed!=0){
        trajectory_compensator_->velocity = bullet_speed;
      }
   }
 }
 */

  
  if(is_outpost){
    all_target_center.update(target_position);
  }

  if(is_outpost&&all_target_center.size()==all_target_center.getMaxSize())
  {
    target_position.x() = all_target_center.getMeanOfNearestPoints().x();
    target_position.y() = all_target_center.getMeanOfNearestPoints().y();
    target_position.z() = all_target_center.getMeanOfNearestPoints().z();
  }
  

  if(is_outpost)
  {
      trajectory_compensator_->velocity = 28.0;
  }
  else
  {
      trajectory_compensator_->velocity = normal_v;
  }
  

  double delay = prediction_delay_;
  if(!last_fire)
  {
    delay += 0.012;
  }

  double flying_time = trajectory_compensator_->getFlyingTime(target_position);
  double dt =
    (current_time - rclcpp::Time(target.header.stamp)).seconds() + flying_time + delay;
  target_position.x() += dt * target.velocity.x;
  target_position.y() += dt * target.velocity.y;
  //target_position.z() += dt * target.velocity.z;
  target_yaw += dt * (target.v_yaw);
  target_center.update(target_position);

  // Choose the best armor to shoot
  std::vector<Eigen::Vector3d> armor_positions = getArmorPositions(
    target_position, target_yaw, target.radius_1, target.radius_2, target.dz, target.armors_num);
  fire_target = armor_positions;
  
  int idx =
    selectBestArmor(armor_positions, target_position, target_yaw, target.v_yaw, target.armors_num);
  //int idx =
    //selectHighSpeedArmor(armor_positions,target_position,target_yaw,target.v_yaw,4,dt);
  index = idx;
  auto chosen_armor_position = armor_positions.at(idx);
  if (chosen_armor_position.norm() < 0.1) {
    throw std::runtime_error("No valid armor to shoot");
  }

  // Calculate yaw, pitch, distance
  double yaw, pitch;
  calcYawAndPitch(chosen_armor_position, rpy, yaw, pitch,target.v_yaw);
  double distance = chosen_armor_position.norm();



  
  // Initialize gimbal_cmd
  rm_interfaces::msg::GimbalCmd gimbal_cmd;
  gimbal_cmd.header = target.header;
  gimbal_cmd.distance = distance;
  gimbal_cmd.fire_advice = isOnTarget(rpy[2], rpy[1], yaw, pitch, distance,target_position,target.v_yaw,target_yaw);
  want_yaw = yaw;
  want_pitch = pitch;




      // 根据转速计算出的限制，调整云台的角度
  double shooting_max_angle = 360.0;
  double shooting_min_angle = -360.0;

  if(target.v_yaw > 1.5){
    double before_yaw = yaw;
    double max_angle = std::abs(atan2(shooting_range_w_*1.5, distance));  // 最大可调整角度
    double min_angle = 0.0;  // 最小可调整角度（或根据具体需求设定）
    // 计算云台可移动的角度限制
    double yaw_limit = calculateYawLimit(target.v_yaw, max_v_yaw, min_v_yaw, max_angle, min_angle);

    // 射击方向的角度
    double shooting_angle = std::atan2(target_position.y(), target_position.x()); 

    // 根据转速计算出的限制，调整云台的角度
    shooting_max_angle = shooting_angle + yaw_limit;
    shooting_min_angle = shooting_angle - yaw_limit;

    yaw = std::clamp(yaw, shooting_min_angle, shooting_max_angle);
    if(yaw !=before_yaw)
    {
      double angle_rad = std::abs(yaw - before_yaw);  // 角度差（需转换为弧度）
      double h = distance; // 举例：可替换为你的目标中心高度或某段距离
      double b = 2 * h * std::tan(angle_rad / 2.0); // 计算对应底边长
      double threshold = 0.135; // 你设定的最小底边长度触发阈值

        if (b < threshold)
        {
          gimbal_cmd.fire_advice=true;// 满足角度变化与建议开火条件，可以执行某些逻辑
        }
    }
    pitch = adjustPitchWithSpeed(pitch, target.v_yaw);
   
  }

  /*
  if(is_outpost)
  {
    double shooting_range_yaw = std::abs(atan2(shooting_range_w_, distance));
    double shooting_angle = std::atan2(target_position.y(), target_position.x()); // 射击方向的角度
    double shooting_max_angle = shooting_angle+shooting_range_yaw;
    double shoooting_min_angle = shooting_angle-shooting_range_yaw;

    if(yaw>=shooting_max_angle)
    {
      yaw = shooting_max_angle;
    }

    if(yaw<=shoooting_min_angle)
    {
      yaw = shoooting_min_angle;
    }

  }
  */



  switch (state) {
    case TRACKING_ARMOR: {
      if (std::abs(target.v_yaw) > max_tracking_v_yaw_) {
        overflow_count_++;
      } else {
        overflow_count_ = 0;
      }

      if (overflow_count_ > transfer_thresh_) {
        state = TRACKING_CENTER;
      }

      // If isOnTarget() never returns true, adjust controller_delay to force the gimbal to move
      if (controller_delay_ != 0) {
        if(!is_outpost){
          target_position.x() += controller_delay_ * target.velocity.x;
          target_position.y() += controller_delay_ * target.velocity.y;
          target_position.z() += controller_delay_ * target.velocity.z;
          target_yaw += controller_delay_ * target.v_yaw;
        }
        armor_positions = getArmorPositions(target_position,
                                            target_yaw,
                                            target.radius_1,
                                            target.radius_2,
                                            target.dz,
                                            target.armors_num);
        shoot_target = armor_positions;
        chosen_armor_position = armor_positions.at(idx);
        if (chosen_armor_position.norm() < 0.1) {
          throw std::runtime_error("No valid armor to shoot");
        }
        calcYawAndPitch(chosen_armor_position, rpy, yaw, pitch,target.v_yaw);
        if(target.v_yaw > 1.5){
          yaw = std::clamp(yaw, shooting_min_angle, shooting_max_angle);
          pitch = adjustPitchWithSpeed(pitch, target.v_yaw);}
      }
      break;
    }
    case TRACKING_CENTER: {
      if (std::abs(target.v_yaw) < max_tracking_v_yaw_) {
        overflow_count_++;
      } else {
        overflow_count_ = 0;
      }

      if (overflow_count_ > transfer_thresh_) {
        state = TRACKING_ARMOR;
        overflow_count_ = 0;
      }
      gimbal_cmd.fire_advice = true;
      if(target_center.size()==target_center.getMaxSize())
      {
        calcYawAndPitch(target_center.getMeanOfNearestPoints(), rpy, yaw, pitch,target.v_yaw);
        break;
      }
      calcYawAndPitch(target_position, rpy, yaw, pitch,target.v_yaw);
      break;
    }
  }
  gimbal_cmd.yaw = yaw * 180 / M_PI;
  gimbal_cmd.pitch = pitch * 180 / M_PI;
  gimbal_cmd.yaw_diff = (yaw - rpy[2]) * 180 / M_PI;
  gimbal_cmd.pitch_diff = (pitch - rpy[1]) * 180 / M_PI;

  if (gimbal_cmd.fire_advice) {
    FYT_DEBUG("armor_solver", "You Need Fire!");
  }
  return gimbal_cmd;
}

bool Solver::isOnTarget(const double cur_yaw,
                        const double cur_pitch,
                        const double target_yaw,
                        const double target_pitch,
                        const double distance,
                        const Eigen::Vector3d target_position,
                        const double v_yaw,
                        const double ekf_yaw)  noexcept {
  
  // Judge whether to shoot
  double shooting_range_yaw = std::abs(atan2(shooting_range_w_ / 2, distance));
  double shooting_range_pitch = std::abs(atan2(shooting_range_h_ / 2, distance));

  double shooting_angle = std::atan2(target_position.y(), target_position.x()); // 射击方向的角度
  double armor_normal_angle = target_yaw; // 装甲板法向量的角度
    
    // 计算夹角的绝对值(归一化到[0,π/2])
    double angle_diff = std::abs(armor_normal_angle - shooting_angle);
    while(angle_diff > M_PI) {
        angle_diff -= 2 * M_PI;
    }
    angle_diff = std::abs(angle_diff);
    if(angle_diff > M_PI/2) {
        angle_diff = M_PI - angle_diff;
    }
    /*
    // 设置允许的最大夹角(比如30度)
    const double MAX_ACCEPTABLE_ANGLE = 10.0 * M_PI / 180.0;
    bool angle_valid = angle_diff < MAX_ACCEPTABLE_ANGLE;*/
  //std::cout<<"is_shotting:"<<is_shotting<<std::endl;

  // Limit the shooting area to 1 degree to avoid not shooting when distance is
  // too large
  shooting_range_yaw = std::max(shooting_range_yaw, 0.0 * M_PI / 180);
  shooting_range_pitch = std::max(shooting_range_pitch, 0.0 * M_PI / 180);


  //std::cout<<"ekf_yaw:"<<ekf_yaw<<std::endl;
  bool is_in_ekf_yaw = false;
  (void)is_in_ekf_yaw;
  if(v_yaw>0){
    if(ekf_yaw<0)
    {
      is_in_ekf_yaw = true;
    }
  }
  else{
    if(ekf_yaw>0)
    {
      is_in_ekf_yaw = true;
    }
  }
  
  //std::cout<<"is_in_ekf_yaw:"<<is_in_ekf_yaw<<std::endl;

  if (//std::abs(cur_yaw - target_yaw)<0.027&&std::abs(cur_yaw - target_yaw)>0.023){
    std::abs(cur_yaw - target_yaw) < shooting_range_yaw){
      //&&std::abs(cur_pitch - target_pitch) < shooting_range_pitch&&angle_valid){
      //is_in_ekf_yaw) {
        last_fire = true;
    return true;
  }
  last_fire = false;
  return false;
}

std::vector<Eigen::Vector3d> Solver::getArmorPositions(const Eigen::Vector3d &target_center,
                                                       const double target_yaw,
                                                       const double r1,
                                                       const double r2,
                                                       const double dz,
                                                       const size_t armors_num) const noexcept {
  auto armor_positions = std::vector<Eigen::Vector3d>(armors_num, Eigen::Vector3d::Zero());
  // Calculate the position of each armor
  bool is_current_pair = true;
  double r = 0., target_dz = 0.;
  for (size_t i = 0; i < armors_num; i++) {
    double temp_yaw = target_yaw + i * (2 * M_PI / armors_num);
    if (armors_num == 4) {
      r = is_current_pair ? r1 : r2;
      target_dz = is_current_pair ? 0 : dz;
      is_current_pair = !is_current_pair;
    } else {
      r = r1;
      target_dz = dz;
    }
    armor_positions[i] =
      target_center + Eigen::Vector3d(-r * cos(temp_yaw), -r * sin(temp_yaw), target_dz);
  }
  return armor_positions;
}

 // armor_solver.cpp

 
int Solver::selectHighSpeedArmor(const std::vector<Eigen::Vector3d>& poses,
                                 const Eigen::Vector3d& center,
                                 double raw_theta,
                                 double raw_omega,
                                 size_t armors_num,
                                 double dt) noexcept {
  // —— 1. 二阶前馈预测 ——  
  double theta_pred = raw_theta;
  double alpha_acc = (raw_omega - last_omega_) / dt;
  theta_pred += 0.5 * alpha_acc * dt * dt;
  last_omega_ = raw_omega;

  // —— 2. 用旋转矩阵计算相对预测角 ——  
  double alpha = std::atan2(center.y(), center.x());                            
  Eigen::Matrix2d R_odom2center, R_odom2pred;                                   
  R_odom2center << std::cos(alpha),  std::sin(alpha),
                   -std::sin(alpha), std::cos(alpha);                            
  R_odom2pred   << std::cos(theta_pred),  std::sin(theta_pred),
                  -std::sin(theta_pred), std::cos(theta_pred);                   

  Eigen::Matrix2d R_center2pred = R_odom2center.transpose() * R_odom2pred;      
  double rel_angle = -std::asin(R_center2pred(0, 1));                            
  const double TWO_PI = 2.0 * M_PI;
  rel_angle = std::fmod(rel_angle, TWO_PI);
  if (rel_angle < 0) rel_angle += TWO_PI;

  // —— 3. 扇区计算 & 滞回 + 编号方向修正 ——  
  double sector = TWO_PI / static_cast<double>(armors_num);
  int idx_raw = static_cast<int>(rel_angle / sector);

  //  若为逆时针旋转，则编号反向映射
  if (raw_omega > 0) {
    idx_raw = (armors_num - idx_raw) % armors_num;
  }

  bool high_speed = std::abs(raw_omega) > omega_thresh_;

  if (last_idx_ < 0) {
    last_idx_   = idx_raw;
    hold_count_ = 0;
    return last_idx_;
  }

  double hysteresis_ = 0.5;

  if (idx_raw != last_idx_) {
    double center_ang = (idx_raw + 0.5) * sector;
    double delta      = std::abs(rel_angle - center_ang);
    if (delta > (sector * 0.5 + hysteresis_)) {
      if (!high_speed || (++hold_count_ >= hold_thresh_)) {
        last_idx_   = idx_raw;
        hold_count_ = 0;
      }
    }
  } else {
    hold_count_ = 0;
  }

  return last_idx_;
}



int Solver::selectBestArmor(const std::vector<Eigen::Vector3d> &armor_positions,
                            const Eigen::Vector3d &target_center,
                            const double target_yaw,
                            const double target_v_yaw,
                            const size_t armors_num) const noexcept {
  // Angle between the car's center and the X-axis
  double alpha = std::atan2(target_center.y(), target_center.x());


  double beta = target_yaw;
  
  //double beta = std::atan2(armor_positions[0].y(), armor_positions[0].x());

  // Angle between the front of observed armor and the X-axis

  
  // clang-format off
  Eigen::Matrix2d R_odom2center;
  Eigen::Matrix2d R_odom2armor;
  R_odom2center << std::cos(alpha), std::sin(alpha), 
                  -std::sin(alpha), std::cos(alpha);
  R_odom2armor << std::cos(beta), std::sin(beta), 
                 -std::sin(beta), std::cos(beta);
  // clang-format on
  Eigen::Matrix2d R_center2armor = R_odom2center.transpose() * R_odom2armor;

  // Equal to (beta - alpha) in most cases
  double decision_angle = -std::asin(R_center2armor(0, 1));

  // Angle thresh of the armor jump
  double theta = (target_v_yaw > 0 ? side_angle_ : -side_angle_) / 180.0 * M_PI;
  if(is_outpost)
  {
      theta = (target_v_yaw > 0 ? 15.0 : -15.0) / 180.0 * M_PI;
  }
  else{
      theta = (target_v_yaw > 0 ? side_angle_ : -side_angle_) / 180.0 * M_PI;
  }

  // Avoid the frequent switch between two 
  
  if (std::abs(target_v_yaw) < min_switching_v_yaw_) {
    theta = 0;
  }

  double temp_angle = decision_angle + M_PI / armors_num - theta;

  if (temp_angle < 0) {
    temp_angle += 2 * M_PI;
  }

  int selected_id = static_cast<int>(temp_angle / (2 * M_PI / armors_num));
  return selected_id;
}


/*
int Solver::selectBestArmor(const std::vector<Eigen::Vector3d> &armor_positions,
                            const Eigen::Vector3d &target_center,
                            const double target_yaw,
                            const double target_v_yaw,
                            const size_t armors_num) const noexcept {
  // 检查目标中心是否为零向量，避免除零错误
  if (target_center.x() == 0.0 && target_center.y() == 0.0) {
    RCLCPP_WARN(this->get_logger(), "目标中心位置为零，使用默认值");
    // 如果目标中心为零，设定一个默认角度
    return 0;  // 默认返回第一个装甲位置
  }

  // 计算目标中心和X轴的角度
  double alpha = std::atan2(target_center.y(), target_center.x());

  // 使用 target_yaw 作为装甲的角度
  double beta = target_yaw;

  // 计算旋转矩阵
  Eigen::Matrix2d R_odom2center;
  Eigen::Matrix2d R_odom2armor;
  R_odom2center << std::cos(alpha), std::sin(alpha), 
                  -std::sin(alpha), std::cos(alpha);
  R_odom2armor << std::cos(beta), std::sin(beta), 
                 -std::sin(beta), std::cos(beta);

  Eigen::Matrix2d R_center2armor = R_odom2center.transpose() * R_odom2armor;

  // 使用 std::clamp 确保传递给 asin 的值在 [-1, 1] 范围内
  double decision_angle = -std::asin(std::clamp(R_center2armor(0, 1), -1.0, 1.0));

  // 根据目标的偏航速度计算角度阈值
  double theta = (target_v_yaw > 0 ? side_angle_ : -side_angle_) / 180.0 * M_PI;
  if (is_outpost) {
      theta = (target_v_yaw > 0 ? 15.0 : -15.0) / 180.0 * M_PI;
  } else {
      theta = (target_v_yaw > 0 ? side_angle_ : -side_angle_) / 180.0 * M_PI;
  }

  // 当目标的偏航速度小于阈值时，避免频繁切换
  if (std::abs(target_v_yaw) < min_switching_v_yaw_) {
    theta = 0;
  }

  // 计算临时角度并调整至 [0, 2π)
  double temp_angle = decision_angle + M_PI / armors_num - theta;
  if (temp_angle < 0) {
    temp_angle += 2 * M_PI;
  }

  // 根据角度选择装甲索引
  int selected_id = static_cast<int>(std::round(temp_angle / (2 * M_PI / armors_num)));
  
  // 确保索引在有效范围内
  selected_id = std::min(selected_id, static_cast<int>(armors_num - 1));

  return selected_id;
}
*/



/*
int Solver::selectBestArmor(const std::vector<Eigen::Vector3d> &armor_positions,
  const Eigen::Vector3d &target_center,
  const double target_yaw,
  const double target_v_yaw,
  const size_t armors_num) const noexcept {
// Angle between the car's center and the X-axis
double alpha = std::atan2(target_center.y(), target_center.x());
//我认为错误的地方，这里不应该使用target_yaw，而是使用armor_positions【0】解算出来的yaw
//double beta = target_yaw;
double beta = std::atan2(armor_positions[0].y(), armor_positions[0].x());
// Angle between the front of observed armor and the X-axis

// clang-format off
Eigen::Matrix2d R_odom2center;
Eigen::Matrix2d R_odom2armor;
R_odom2center << std::cos(alpha), std::sin(alpha), 
-std::sin(alpha), std::cos(alpha);
R_odom2armor << std::cos(beta), std::sin(beta), 
-std::sin(beta), std::cos(beta);
// clang-format on
Eigen::Matrix2d R_center2armor = R_odom2center.transpose() * R_odom2armor;

// Equal to (beta - alpha) in most cases
double decision_angle = std::abs(std::asin(R_center2armor(0, 1)));
int selected_id = 0;
if(target_v_yaw>0&&decision_angle>10){
  selected_id = 3;
}
else if(target_v_yaw<0&&decision_angle<-10)
{
  selected_id = 1;
}

return selected_id;
}

*/

double Solver::calculateYawLimit(double enemy_v_yaw, double max_v_yaw, double min_v_yaw, 
                         double max_angle, double min_angle) noexcept {
  // 计算敌方转速与云台角度的反比关系
  double normalized_v_yaw = std::clamp(enemy_v_yaw, min_v_yaw, max_v_yaw);  // 限制在最大和最小转速之间
  double scale = (max_v_yaw - normalized_v_yaw) / (max_v_yaw - min_v_yaw);  // 反比缩放因子
  double yaw_limit = min_angle + scale * (max_angle - min_angle);  // 计算云台的角度限制
  return yaw_limit;
}

double Solver::adjustPitchWithSpeed(double pitch, double v_yaw, double k, double base_speed, double max_compensate) noexcept {
    if (v_yaw <= base_speed) return pitch;

    double delta = k * std::log1p(v_yaw - base_speed);  // log1p(x) = log(1 + x)
    delta = std::min(delta, max_compensate);
    return pitch - delta;
}

void Solver::calcYawAndPitch(const Eigen::Vector3d &p,
                             const std::array<double, 3> rpy,
                             double &yaw,
                             double &pitch,const double &v_yaw) const noexcept {
  // Calculate yaw and pitch
  yaw = atan2(p.y(), p.x());
  pitch = atan2(p.z(), p.head(2).norm());

  if (double temp_pitch = pitch; trajectory_compensator_->compensate(p, temp_pitch,v_yaw)) {
    pitch = temp_pitch;
  }
}
}  // namespace fyt::auto_aim
